package r2d2.robot;

import lejos.nxt.Motor;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import r2d2.actuators.AgentPcConnection;
import r2d2.behavior.CollisionDetector;
import r2d2.behavior.Move;
import r2d2.sensors.Sensors;


public class Robot {
	
	private Sensors sensors;
	private TachoPilot pilot;
	private Arbitrator arbitrator;
	private static float WHEEL_DIAMETER = 56f;
	private static float TRACK_DIMENSION = 107f;
	private boolean inCollision = false;
	private AgentPcConnection connection;
	
	public Robot() {
		
		
		
		//MOVMENTE HANDLER
		pilot = new TachoPilot(WHEEL_DIAMETER, TRACK_DIMENSION, Motor.C, Motor.A);
		//INIT SENSORE
		sensors = new Sensors();
		//INIT BEHAVIOR
		Behavior lists[] = { new Move(this), new CollisionDetector(this)};
		arbitrator = new Arbitrator(lists);
		RobotUtility.die();
		
		
		//WRAPPER PC CONNECTION
		connection = new AgentPcConnection();
		
	}
	
	public synchronized boolean isInCollision() {
		return inCollision;
	}
	public synchronized void setInCollision(boolean inCollision) {
		this.inCollision = inCollision;
	}
	
	
	/**
	 * BEHAVIOR MANAGER START
	 */
	public void start() {
		arbitrator.start();
	}


 

	
	public Sensors getSensors() {
		return sensors;
	}
	public TachoPilot getPilot() {
		return pilot;
	}
	public Arbitrator getArbitrator() {
		return arbitrator;
	}
	public AgentPcConnection getConnection() {
		return connection;
	}
}
